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Estimating Rigid Transformation Between Two 
Range Maps Using Expectation Maximization 

Algorithm 



Shuqing Zeng^ 



Abstract 

We address the problem of estimating a rigid transformation between two point sets, which is a key 
module for target tracking system using Light Detection And Ranging (LiDAR). A fast implementation 
of Expectation-maximization (EM) algorithm is presented whose complexity is 0{N) with N the number 
of scan points. 

I. INTRODUCTION 

Rigid registration of two sets of points sampled from a surface has been widely investigated (e.g., HI, 
1111-0, HI) in computer vision literature. Generally, these methods are designed to tackle range maps 
with dense points for non-realtime applications. 

In fill, fSl] scans are matched using iterative closest line (ICL), a variant of "normal-distance" form of 
ICP algorithm [1] originally proposed in computer vision community by [3|. However, the convergence 
of this approach is sensitive to errors in normal direction estimations |[TOl . 




Fig. 1. Illustration of the proposed algorithm. 

Fig. [U illustrates the concept. The light green circles denote the contour of a target M.. The red circles 
are the projection of M. under a rigid transformation T, denoted as M. Let S be the current range image 
shown as upper triangles. We propose an Expectation-maximization (EM) algorithm Pl, fT| to find the 
rigid transformation such that the projected range image best matches the current image. Each point nij 
in M is treated as the center of a Parzen window. There is an edge between G 5 and rrij if Sk hes in 
the window. The weight of the edge {sk,mj) is based on the proximity between the two vertices. The 
larger weight of the edge, the thicker the line is shown, and the more force that pulls the corresponding 
the point rrij to Sk through T. 

This document describes a fast implementation of expectation maximization (EM) algorithm Q to 
locally match between M and S. By exploiting the sparsity of the locally matching matrix, this imple- 
mentation scales linearly with the number of points. 
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II. ALGORITHM DERIVATION 

This section is devoted to the problem of how to estimate the rigid transformation T using (EM) 
algorithm, giving scan map S and a contour model M.. 

We constructed a bipartite graph B = [M, S, Eb) between the vertex set M.io S with Eb the set of 
edges. Let m ^ M. and s G <S. An edge exists between the points s and m if and only if ||s — m|| < W 
with W a distance threshold. By M{s) = {m \ {s,m) G Eb} we denote the neighborhood of s. 

Scan points are indexed using a lookup hash-table with W/2 resolution. Find the points m near a 
point s within the radius W involving searching through all the three-by-three neighbor grid of the cell 
containing s. Since hash table is used, and \J\f{s) \ is bounded, construction graph B is an 0{N) operation 
with N the number of points in a scan. 

Let Sj G 5 be one of the ns scan points, and nik G be one of the nj^ points from the model. We 
denote T a rigid transformation from the model to the new scan frame, with the parameter vector y. If 
Sj is the measure of ruk (i.e., {sj, ruk) G B) with a known noise model, we write the density function as 
p{sj I rrife, y) = p{sj \ T{mk,y))- In case of an additive and centered Gaussian noise of precision matrix 
r, p{sj I mk,T) = cexp(— ^ where the Mahalanobis norm is defined as 1|.t||p = x^Tx. 

We use the binary matrix A to represent the correspondence between Sj and mfe. The entry Aj^ = 1 
if Sj matches and otherwise. Assume each scan point Sj corresponds to at most one model point. 
We have 



1 \fM{sj)i^ 
^^^^ ~ \ Otherwise. 

for all scan point index j. 

For the above equation, we note that for the case M{sj) = 0, Sj is an outlier, and the correspondence 
Sj to mj. can be treated as a categorical distribution. In order to apply EM procedure we use a random 
matching matrix A with each element a binary random variable. Each eligible matching matrix A has 
a probability p{A) = p{A = A). One can verify that Ajk = E{Ajk} = P{Ajk = 1), and the following 
constraint holds 



^k^jk - I Q Otherwise. 



Considering the distribution of Aj, the j-th row of the A, which is the distribution of assigning the 
scan point Sj to the model point ruk, i.e., 

piA,)= n 

Assuming the scan points are independent, we can write 

f (^) = n n (^^k)^'' = n ^^^k)-^'' 

An example of p{A) is the noninformative prior probability of the matches: a probability distribution 
that a given scan point is a measure of a given model point without knowing measurement information: 

^ \ Otherwise. 

The joint probability of the scan point Sj and the corresponding assignment Aj can be expressed as 

p{sj,Aj \M,y) = Yl i^jkPi^j \mk,y))^"' 

mkeM{sj) 
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Providing that the scan points are conditionally independent, the overall joint probability is the product 
of the each row of A: 

p{S,A\M,y)= J] {TTjkP{sj \ mk,y))^^'' (2) 
and the logarithm of marginal distribution can be written as 

ML(r) = logp{S \M,y)=log (^p{S,A\ M,y)j (3) 

Unfortunately, Eq. (|3]) has no closed-form solution and no robust and efficient algorithm to directly 
minimize it with respect to the parameter y. Noticing that Eq. ^ only involves the logarithm of a sum, 
we can treat the matching matrix A as latent variables and apply the EM algorithm to iteratively estimate 
y. Assuming after n-th iteration, the current estimate for y is given by y„, we can compute an updated 
estimate T such that ML(T) is monotonically increasing, i.e., 

A(y I y„,) = ML(y) - ML(y„) > 

Namely, we want to maximize the difference A(y | y„). 

Now we are ready to state two propositions whose proofs are relegated to Appendix. 
Proposition 2.1: 

A(y 1 yn) = E^|5,^,y Jlog {p{S,A I M,y))} 

Proposition 2.2: Given the transformation estimate y„, scan points S and model points ^A, the 
posterior of the matching matrix A can be written as 

p{A\S,M,yn)= n (^i'^)"^'" (4) 

where 

I Otherwise. 

Therefore, we have the following EM algorithm to compute y that maximizes the likelihood defined in 
Eq. ([3]). We assume there exists an edge in the graph B between sj and ruk in the following derivation. 

• E-step: Given the previous estimate T^, we update Ajk using Eq. (O. The conditional expectation 
is computed as 

A(y I yn) = ^A\SM,y„ {^ogp{S,A \ M,y)} 
= E jlog ^J|7rj7cP(sj I mk^y)-^'" 
= '^E{Ajk} {log p{s J I mk,y) + \ogTTjk) 

= ^A.jk\\sj - T{mk,y)\\l + const. (6) 

where E is Eji,\s,M,y„ short, and const, is the terms irrelevant to y. 

• M-step: Compute y to maximize the least-squares expression in Eq. (|6j. 

The above EM procedure is repeated until the model is converged, i.e., the difference of log-likelihood 
between two iterations A(y | y„) is less than a small number. The complexity of the above computation 
for a target in each iteration is 0{\Es\). Since the number of neighbors for Sj is bounded, the complexity 
is reduced to 0(|5|). Since experimental result shows that only 4-5 epochs are needed for EM iteration 
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to converge. Consequently, the overall complexity for all of the tracked objects is 0{N) with N the 
number of scan points. 

The following proposition shows how to compute the covariance matrix for the transformation param- 
eters y. 

Proposition 2.3: Given y, the covariance matrix R is 

R=— Ajk{sj -T{mk,y)){sj -T{mk,y)f (7) 

np ^-^ 

where np is the number of the nonzero rows of the matrix A. 

III. Proof of Propositions 

A. Proof of Proposition 2.1 



A(y I yn) = ML(y) - ML(y, 



log (^p{S,A\M,y)^ - log {p{S I M,yn)) 



log iJ2piS\A,M,y)p{A\M,y) 

\ A 

logpiS\M,yn) 

^p{S\A,M,y)p{A\M,y) 

logp{S\M,yn) 



<Z> Jr \~ |- - 7 t/ 111 / 

1 ( A\Q KA ^ P{S\A,M,y)p{ 
log ^}^piA\S,M,y^) ^^j^^ 



A 

logp{S\M,yn) (8) 



V- ^l ( p{S\A,M,y)p{A\M,y) 

> p^5,7W,y„ log woX^ ^ 

^ \p{A\S,M,yn)p{S\M,yn) 



A 

where Jansen's inequality and convexity of logarithm function are applied in deriving Eq. dH). Since we 
are maximizing A(y|y„) with respect to y, we can drop terms that are irrelevant to y, thus 

A(y |y„) = Y.p{A\S,M,yn) log {p[S\A,M.y)p{A\M, y)) 

A 

\- ^yiic KA ^^ ( p{S,A,y \M)p{A,y\M) 
= ^p{A\S, M,yn)log [-^^ 



\M) p{y\M) 
p{S,A,y\My 



A 

Y,p{A\s M,yn)iog 

A 

P{A\S M,yn) log {p{S, A\M,y)) 



p(y\M) 



E^|5,Ai,y„{log(p(5,^lA^,y))} (9) 
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B. Proof of Proposition 2.2 

If M{sj) 7^ 0, the marginal PDF of the j-th row of A is p{sj\M-,'y) = ^^jkPisj [m-fc, y). We assume 
there exists an edge in the graph B between the scan and model points sj and m^, and scan points are 
independent each other. One can verify that 

p{S\M,yn) = '[l\^7Tjkp{s 

i I f^k ) Yn ) 1 

Using Bayesian theorem, we have 

^/iic ^ p{S,A\M,yn) 

p[A\S,M,yn) = — T^TT-j ^ 

p{S\M,yn) 

_ Hj.fc i'^jkP{sj\mk,yn))-^"' 

Uj iT.k^jkPiSj\mk,yn)) 

_ Uj,k i'^jkPisj\mk,yn))-^"' 

I\j,k (Efc TTjkPiSjlrUk, yn))-^'" 

_Y\ f '^jkP{sj\mk,yn) \ 
y \^k^jkP{sj\mk,yn)J 

Comparing with Eq. dJ]), the equation Eq. (jS) holds. 

C. Proof of Proposition 2.3 

We treat the precision matrix T as the uncertainty of unknown transformation parameter y. We 
use a maximum likelihood approach, which amounts to minimizing Eq. ^ with respect to T given 
a transformation and a set of matches with probabilities: 

— A(y[y„) 

= 1 E i,,( ii'^-^';"-^)ii? +iog|r|-i 



I Ajkisj -T{mk,y)){sj -T{mk,y)f 



2 

where np is the number of nonzero rows of the matrix A. Thereby the covariance matrix R is computed 

as 



R = r 



-1 



— ^jk{sj -T{mk)){sj -T{mk)y 



np 

{sj,mk)&EB 
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